#include <memory>
#include <string>
#include <vector>
#include <fstream>
#include <sstream>
#include <unordered_map>
#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <tf2_ros/static_transform_broadcaster.h>

class CalibTFBroadcaster : public rclcpp::Node
{
public:
    CalibTFBroadcaster()
    : Node("calib_tf_broadcaster")
    {
        static_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);
        bool loaded = load_yaml_params("gps_calibration_params.yaml");

        if (loaded) {
            publish_static_tf(offset_x_, offset_y_);
            last_tx_ = offset_x_;
            last_ty_ = offset_y_;
            RCLCPP_INFO(this->get_logger(), "已根据gps_calibration_params.yaml发布body->gps静态TF，tx=%.3f, ty=%.3f", offset_x_, offset_y_);
        } else {
            RCLCPP_WARN(this->get_logger(), "未找到gps_calibration_params.yaml或解析失败，等待订阅到数据后再广播TF。");
        }

        result_sub_ = this->create_subscription<geometry_msgs::msg::TransformStamped>(
            "extrinsics", 10,
            std::bind(&CalibTFBroadcaster::result_callback, this, std::placeholders::_1));

        RCLCPP_INFO(this->get_logger(), "标定TF广播节点已启动。");
    }

private:
    rclcpp::Subscription<geometry_msgs::msg::TransformStamped>::SharedPtr result_sub_;
    std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_broadcaster_;
    double offset_x_ = 0.0;
    double offset_y_ = 0.0;
    double last_tx_ = std::numeric_limits<double>::quiet_NaN();
    double last_ty_ = std::numeric_limits<double>::quiet_NaN();
    const double eps_ = 1e-6;

    bool load_yaml_params(const std::string& file_path)
    {
        std::ifstream fin(file_path);
        if (!fin.is_open()) {
            RCLCPP_WARN(this->get_logger(), "无法打开文件: %s", file_path.c_str());
            return false;
        }

        std::string line;
        std::unordered_map<std::string, std::string> param_map;
        while (std::getline(fin, line)) {
            if (line.empty() || line[0] == '#')
                continue;
            auto pos = line.find(':');
            if (pos == std::string::npos)
                continue;
            std::string key = line.substr(0, pos);
            std::string value = line.substr(pos + 1);
            key.erase(0, key.find_first_not_of(" \t"));
            key.erase(key.find_last_not_of(" \t") + 1);
            value.erase(0, value.find_first_not_of(" \t"));
            value.erase(value.find_last_not_of(" \t\r\n") + 1);
            param_map[key] = value;
        }
        fin.close();

        if (param_map.count("gps_init_offset_x") && param_map.count("gps_init_offset_y")) {
            try {
                offset_x_ = std::stod(param_map["gps_init_offset_x"]);
                offset_y_ = std::stod(param_map["gps_init_offset_y"]);
                return true;
            } catch (const std::exception& e) {
                RCLCPP_ERROR(this->get_logger(), "解析gps_init_offset_x/y失败: %s", e.what());
                return false;
            }
        }
        return false;
    }

    void publish_static_tf(double tx, double ty)
    {
        geometry_msgs::msg::TransformStamped static_transform;
        static_transform.header.stamp = this->now();
        static_transform.header.frame_id = "body";
        static_transform.child_frame_id = "gps";
        static_transform.transform.translation.x = tx;
        static_transform.transform.translation.y = ty;
        static_transform.transform.translation.z = 0.0;
        static_transform.transform.rotation.x = 0.0;
        static_transform.transform.rotation.y = 0.0;
        static_transform.transform.rotation.z = 0.0;
        static_transform.transform.rotation.w = 1.0;

        static_broadcaster_->sendTransform(static_transform);
    }

    bool is_same_tf(double tx, double ty)
    {
        return (std::abs(tx - last_tx_) < eps_) && (std::abs(ty - last_ty_) < eps_);
    }

    void result_callback(const geometry_msgs::msg::TransformStamped::SharedPtr msg)
    {
        double tx = msg->transform.translation.x;
        double ty = msg->transform.translation.y;

        // 若和上一次数据相同，不重复广播
        if (is_same_tf(tx, ty)) {
            RCLCPP_DEBUG(this->get_logger(), "接收到的TF与上次相同，跳过广播。");
            return;
        }

        publish_static_tf(tx, ty);
        last_tx_ = tx;
        last_ty_ = ty;
        RCLCPP_INFO(this->get_logger(), "已广播body->gps静态TF，tx=%.3f, ty=%.3f", tx, ty);
    }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<CalibTFBroadcaster>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}